#include "feCgSolver.h"
#include "feVector.h"
#include "feMvOps.h"
#include "feMatrix.h"
#include <math.h>


feCgSolver::feCgSolver()
{
	M = NULL;
	F = NULL;
	X = NULL;
	R = NULL;
	Z0 = NULL;
}


//--------------------------------------------------------------------------------------------------------------------------------
void feCgSolver::BindData(feMatrix *iM, feVector *iF, feVector *iX0)
{
	M = iM;
	F = iF;
	if(X)
		delete X;
	if(Z0)
		delete Z0;
	if(R)
		delete R;
	

	//create vectors
	X = new feVector(iX0->GetSize());
	Z0 = new feVector(iX0->GetSize());
	Z1 = new feVector(iX0->GetSize());
	R = new feVector(iX0->GetSize());
	D = new feVector(iM->GetSize());
	

	//fill d
	for(int i=0; i<D->GetSize(); i++)
		(*D)[i] = *(iM->_IJ(i, i));


	//copy x0
	feMvOps::CopyVV(iX0, X);
	//calc Z, R
	feMvOps::MultMV(M, X, Z0);
	feMvOps::MultSubVV(F, Z0, R);
	feMvOps::DivideVV(R, D, Z0);

	//additional vector
	Z1 = new feVector(Z0->GetSize());
}


//--------------------------------------------------------------------------------------------------------------------------------
void feCgSolver::SetX0(feVector *iX0)
{
	feMvOps::CopyVV(iX0, X);

	//calc Z, R
	feMvOps::MultMV(M, X, Z0);
	feMvOps::MultSubVV(F, Z0, R);
	feMvOps::DivideVV(R, D, Z0);
}


//--------------------------------------------------------------------------------------------------------------------------------
void feCgSolver::Iterate()
{
	//calc alpha top
	feMvOps::DivideVV(R, D, Z1);
	double alphatop = feMvOps::DotVV(Z1, R);
	
	//Mz(k-1)
	feMvOps::MultMV(M, Z0, Z1);
	
	//calc alpha
	double alpha = alphatop/feMvOps::DotVV(Z0, Z1);
	
	//update X
	feMvOps::MultAddVV(X, Z0, X, alpha);

	//update r
	feMvOps::MultSubVV(R, Z1, R, alpha);
	
	//betha
	feMvOps::DivideVV(R, D, Z1);
	double betha = feMvOps::DotVV(Z1, R)/alphatop;
	
	//update z
	feMvOps::MultAddVV(Z1, Z0, Z0, betha);
}


//--------------------------------------------------------------------------------------------------------------------------------
double	feCgSolver::GetEps()
{
	//||f-Ax||
	return sqrt(feMvOps::DotVV(R, R));
}


//--------------------------------------------------------------------------------------------------------------------------------
void feCgSolver::GetX(feVector *oX)
{
	feMvOps::CopyVV(X, oX);
}


//--------------------------------------------------------------------------------------------------------------------------------
feCgSolver::~feCgSolver()
{
	if(X)
		delete X;
	if(Z0)
		delete Z0;
	if(Z1)
		delete Z1;
	if(R)
		delete R;
}
